#pragma once

#include "vec4.h"
#include "box3.h"

OPEN_JLIB_NS


struct alignas(16) Mat4 {

  f32 p[0];
  f32 s00, s01, s02, s03, s10, s11, s12, s13, s20, s21, s22, s23, s30, s31, s32, s33;

  inline Mat4 () noexcept = default;
  inline Mat4 (f32 s00, f32 s10, f32 s20, f32 s30, f32 s01, f32 s11, f32 s21, f32 s31, f32 s02, f32 s12, f32 s22, f32 s32, f32 s03, f32 s13, f32 s23, f32 s33) noexcept
  : s00(s00), s01(s01), s02(s02), s03(s03)
  , s10(s10), s11(s11), s12(s12), s13(s13)
  , s20(s20), s21(s21), s22(s22), s23(s23)
  , s30(s30), s31(s31), s32(s32), s33(s33) {
  }
  inline Mat4 (Vec4 c0, Vec4 c1, Vec4 c2, Vec4 c3) noexcept {
    s00 = c0.x();
    s01 = c0.y();
    s02 = c0.z();
    s03 = c0.w();
    s10 = c1.x();
    s11 = c1.y();
    s12 = c1.z();
    s13 = c1.w();
    s20 = c2.x();
    s21 = c2.y();
    s22 = c2.z();
    s23 = c2.w();
    s30 = c3.x();
    s31 = c3.y();
    s32 = c3.z();
    s33 = c3.w();
  }
  inline Mat4 (const f32* m) noexcept {
    s00 = m[0];
    s01 = m[1];
    s02 = m[2];
    s03 = m[3];
    s10 = m[4];
    s11 = m[5];
    s12 = m[6];
    s13 = m[7];
    s20 = m[8];
    s21 = m[9];
    s22 = m[10];
    s23 = m[11];
    s30 = m[12];
    s31 = m[13];
    s32 = m[14];
    s33 = m[15];
  }


  template<uwl Index>
  inline f32 c () const noexcept {
    return p[Index];
  }


  inline Vec4 operator* (Vec4 v) const noexcept {
    auto x = v.x();
    auto y = v.y();
    auto z = v.z();
    auto w = v.w();
    return {
      s00*x + s10*y + s20*z + s30*w,
      s01*x + s11*y + s21*z + s31*w,
      s02*x + s12*y + s22*z + s32*w,
      s03*x + s13*y + s23*z + s33*w,
    };
  }
  inline Mat4 operator* (const Mat4& m) const noexcept {
    auto t00 = s00*m.s00 + s10*m.s01 + s20*m.s02 + s30*m.s03;
    auto t01 = s01*m.s00 + s11*m.s01 + s21*m.s02 + s31*m.s03;
    auto t02 = s02*m.s00 + s12*m.s01 + s22*m.s02 + s32*m.s03;
    auto t03 = s03*m.s00 + s13*m.s01 + s23*m.s02 + s33*m.s03;
    auto t10 = s00*m.s10 + s10*m.s11 + s20*m.s12 + s30*m.s13;
    auto t11 = s01*m.s10 + s11*m.s11 + s21*m.s12 + s31*m.s13;
    auto t12 = s02*m.s10 + s12*m.s11 + s22*m.s12 + s32*m.s13;
    auto t13 = s03*m.s10 + s13*m.s11 + s23*m.s12 + s33*m.s13;
    auto t20 = s00*m.s20 + s10*m.s21 + s20*m.s22 + s30*m.s23;
    auto t21 = s01*m.s20 + s11*m.s21 + s21*m.s22 + s31*m.s23;
    auto t22 = s02*m.s20 + s12*m.s21 + s22*m.s22 + s32*m.s23;
    auto t23 = s03*m.s20 + s13*m.s21 + s23*m.s22 + s33*m.s23;
    auto t30 = s00*m.s30 + s10*m.s31 + s20*m.s32 + s30*m.s33;
    auto t31 = s01*m.s30 + s11*m.s31 + s21*m.s32 + s31*m.s33;
    auto t32 = s02*m.s30 + s12*m.s31 + s22*m.s32 + s32*m.s33;
    auto t33 = s03*m.s30 + s13*m.s31 + s23*m.s32 + s33*m.s33;
    return {
      t00, t10, t20, t30,
      t01, t11, t21, t31,
      t02, t12, t22, t32,
      t03, t13, t23, t33,
    };
  }


  inline f32 operator* () const noexcept {
    auto a0 = s00*s11 - s01*s10;
    auto a1 = s00*s12 - s02*s10;
    auto a2 = s00*s13 - s03*s10;
    auto a4 = s01*s13 - s03*s11;
    auto b0 = s22*s33 - s23*s32;
    auto b1 = s21*s33 - s23*s31;
    auto b2 = s21*s32 - s22*s31;
    auto b4 = s20*s32 - s22*s30;
    auto a3 = s01*s12 - s02*s11;
    auto a5 = s02*s13 - s03*s12;
    auto b3 = s20*s33 - s23*s30;
    auto b5 = s20*s31 - s21*s30;
    return (a0*b0 - a1*b1) + (a2*b2 - a4*b4) + (a3*b3 + a5*b5);
  }


  inline Mat4 operator~ () const noexcept {
    return {
      s00, s01, s02, s03,
      s10, s11, s12, s13,
      s20, s21, s22, s23,
      s30, s31, s32, s33,
    };
  }
  inline Mat4 operator- () const noexcept {
    auto a0 = s00*s11 - s01*s10;
    auto a1 = s00*s12 - s02*s10;
    auto a2 = s00*s13 - s03*s10;
    auto a4 = s01*s13 - s03*s11;
    auto b0 = s22*s33 - s23*s32;
    auto b1 = s21*s33 - s23*s31;
    auto b2 = s21*s32 - s22*s31;
    auto b4 = s20*s32 - s22*s30;
    auto a3 = s01*s12 - s02*s11;
    auto a5 = s02*s13 - s03*s12;
    auto b3 = s20*s33 - s23*s30;
    auto b5 = s20*s31 - s21*s30;
    auto det = (a0*b0 - a1*b1) + (a2*b2 - a4*b4) + (a3*b3 + a5*b5);

    auto t00 = ( s11*b0 - s12*b1 + s13*b2) / det;
    auto t01 = (-s01*b0 + s02*b1 - s03*b2) / det;
    auto t02 = ( s31*a5 - s32*a4 + s33*a3) / det;
    auto t03 = (-s21*a5 + s22*a4 - s23*a3) / det;

    auto t10 = (-s10*b0 + s12*b3 - s13*b4) / det;
    auto t11 = ( s00*b0 - s02*b3 + s03*b4) / det;
    auto t12 = (-s30*a5 + s32*a2 - s33*a1) / det;
    auto t13 = ( s20*a5 - s22*a2 + s23*a1) / det;

    auto t20 = ( s10*b1 - s11*b3 + s13*b5) / det;
    auto t21 = (-s00*b1 + s01*b3 - s03*b5) / det;
    auto t22 = ( s30*a4 - s31*a2 + s33*a0) / det;
    auto t23 = (-s20*a4 + s21*a2 - s23*a0) / det;

    auto t30 = (-s10*b2 + s11*b4 - s12*b5) / det;
    auto t31 = ( s00*b2 - s01*b4 + s02*b5) / det;
    auto t32 = (-s30*a3 + s31*a1 - s32*a0) / det;
    auto t33 = ( s20*a3 - s21*a1 + s22*a0) / det;

    return {
      t00, t10, t20, t30,
      t01, t11, t21, t31,
      t02, t12, t22, t32,
      t03, t13, t23, t33,
    };
  }


  inline bool inverse (f32 threshold = 0.0000001F) noexcept {
    auto a0 = s00*s11 - s01*s10;
    auto a1 = s00*s12 - s02*s10;
    auto a2 = s00*s13 - s03*s10;
    auto a4 = s01*s13 - s03*s11;
    auto b0 = s22*s33 - s23*s32;
    auto b1 = s21*s33 - s23*s31;
    auto b2 = s21*s32 - s22*s31;
    auto b4 = s20*s32 - s22*s30;
    auto a3 = s01*s12 - s02*s11;
    auto a5 = s02*s13 - s03*s12;
    auto b3 = s20*s33 - s23*s30;
    auto b5 = s20*s31 - s21*s30;
    auto det = (a0*b0 - a1*b1) + (a2*b2 - a4*b4) + (a3*b3 + a5*b5);
    if (fabsf(det) < threshold)
      return false;
    auto t00 = ( s11*b0 - s12*b1 + s13*b2) / det;
    auto t01 = (-s01*b0 + s02*b1 - s03*b2) / det;
    auto t02 = ( s31*a5 - s32*a4 + s33*a3) / det;
    auto t03 = (-s21*a5 + s22*a4 - s23*a3) / det;
    auto t10 = (-s10*b0 + s12*b3 - s13*b4) / det;
    auto t11 = ( s00*b0 - s02*b3 + s03*b4) / det;
    auto t12 = (-s30*a5 + s32*a2 - s33*a1) / det;
    auto t13 = ( s20*a5 - s22*a2 + s23*a1) / det;
    auto t20 = ( s10*b1 - s11*b3 + s13*b5) / det;
    auto t21 = (-s00*b1 + s01*b3 - s03*b5) / det;
    auto t22 = ( s30*a4 - s31*a2 + s33*a0) / det;
    auto t23 = (-s20*a4 + s21*a2 - s23*a0) / det;
    auto t30 = (-s10*b2 + s11*b4 - s12*b5) / det;
    auto t31 = ( s00*b2 - s01*b4 + s02*b5) / det;
    auto t32 = (-s30*a3 + s31*a1 - s32*a0) / det;
    auto t33 = ( s20*a3 - s21*a1 + s22*a0) / det;
    s00 = t00;
    s01 = t01;
    s02 = t02;
    s03 = t03;
    s10 = t10;
    s11 = t11;
    s12 = t12;
    s13 = t13;
    s20 = t20;
    s21 = t21;
    s22 = t22;
    s23 = t23;
    s30 = t30;
    s31 = t31;
    s32 = t32;
    s33 = t33;
    return true;
  }


  inline static Mat4 zero () noexcept {
    return {
      0.0F, 0.0F, 0.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 0.0F,
    };
  }
  inline static Mat4 identity () noexcept {
    return {
      1.0F, 0.0F, 0.0F, 0.0F,
      0.0F, 1.0F, 0.0F, 0.0F,
      0.0F, 0.0F, 1.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 translate (Vec3 p) noexcept {
    return {
      1.0F, 0.0F, 0.0F, p[0],
      0.0F, 1.0F, 0.0F, p[1],
      0.0F, 0.0F, 1.0F, p[2],
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 rotatex (f32 a) noexcept {
    auto s = sinf(a);
    auto c = cosf(a);
    return {
      1.0F, 0.0F, 0.0F, 0.0F,
      0.0F,    c,   -s, 0.0F,
      0.0F,    s,    c, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 rotatey (f32 a) noexcept {
    auto s = sinf(a);
    auto c = cosf(a);
    return {
         c, 0.0F,    s, 0.0F,
      0.0F, 1.0F, 0.0F, 0.0F,
        -s, 0.0F,    c, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 rotatez (f32 a) noexcept {
    auto s = sinf(a);
    auto c = cosf(a);
    return {
         c,   -s, 0.0F, 0.0F,
         s,    c, 0.0F, 0.0F,
      0.0F, 0.0F, 1.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 rotatexp (f32 a, Vec3 p) noexcept {
    auto s = sinf(a);
    auto c = cosf(a);
    auto dy = p[1] - c*p[1] + s*p[2];
    auto dz = p[2] - s*p[1] - c*p[2];
    return {
      1.0F, 0.0F, 0.0F, 0.0F,
      0.0F,    c,   -s,   dy,
      0.0F,    s,    c,   dz,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 rotateyp (f32 a, Vec3 p) noexcept {
    auto s = sinf(a);
    auto c = cosf(a);
    auto dx = p[0] - c*p[0] - s*p[2];
    auto dz = p[2] + s*p[0] - c*p[2];
    return {
         c, 0.0F,    s,   dx,
      0.0F, 1.0F, 0.0F, 0.0F,
        -s, 0.0F,    c,   dz,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 rotatezp (f32 a, Vec3 p) noexcept {
    auto s = sinf(a);
    auto c = cosf(a);
    auto dx = p[0] - c*p[0] + s*p[1];
    auto dy = p[1] - s*p[0] - c*p[1];
    return {
         c,   -s, 0.0F,   dx,
         s,    c, 0.0F,   dy,
      0.0F, 0.0F, 1.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 rotatev (f32 a, Vec3 v) noexcept {
    auto s = sinf(a);
    auto c = 1.0F - cosf(a);
    auto x = v[0];
    auto y = v[1];
    auto z = v[2];
    auto sx = s * x;
    auto sy = s * x;
    auto sz = s * x;
    auto cx = c * x;
    auto cy = c * y;
    auto cz = c * z;
    auto cxx = cx * x;
    auto cxy = cx * y;
    auto cxz = cx * z;
    auto cyy = cy * y;
    auto cyz = cy * z;
    auto czz = cz * z;
    return {
       cxx+c, cxy-sz, cxz+sy, 0.0F,
      cxy+sz,  cyy+c, cyz-sx, 0.0F,
      cxz-sy, cyz+sx,  czz+c, 0.0F,
        0.0F,   0.0F,   0.0F, 1.0F,
    };
  }
  inline static Mat4 rotatevp (f32 a, Vec3 v, Vec3 p) noexcept {
    auto s = sinf(a);
    auto c = 1.0F - cosf(a);
    auto x = v[0];
    auto y = v[1];
    auto z = v[2];
    auto sx = s * x;
    auto sy = s * x;
    auto sz = s * x;
    auto cx = c * x;
    auto cy = c * y;
    auto cz = c * z;
    auto cxx = cx * x;
    auto cxy = cx * y;
    auto cxz = cx * z;
    auto cyy = cy * y;
    auto cyz = cy * z;
    auto czz = cz * z;
    auto s00 = cxx + c;
    auto s01 = cxy + sz;
    auto s02 = cxz - sy;
    auto s10 = cxy - sz;
    auto s11 = cyy + c;
    auto s12 = cyz + sx;
    auto s20 = cxz + sy;
    auto s21 = cyz - sx;
    auto s22 = czz + c;
    auto dx = p[0] - s00*p[0] - s10*p[1] - s20*p[2];
    auto dy = p[1] - s01*p[0] - s11*p[1] - s21*p[2];
    auto dz = p[2] - s02*p[0] - s12*p[1] - s22*p[2];
    Mat4 r = {
       cxx+c, cxy-sz, cxz+sy,   dx,
      cxy+sz,  cyy+c, cyz-sx,   dy,
      cxz-sy, cyz+sx,  czz+c,   dz,
        0.0F,   0.0F,   0.0F, 1.0F,
    };
  }
  inline static Mat4 scale (Vec3 t) noexcept {
    return {
      t[0], 0.0F, 0.0F, 0.0F,
      0.0F, t[1], 0.0F, 0.0F,
      0.0F, 0.0F, t[2], 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 scalep (Vec3 t, Vec3 p) noexcept {
    auto dx = p[0] - t[0]*p[0];
    auto dy = p[1] - t[1]*p[1];
    auto dz = p[2] - t[2]*p[2];
    return {
      t[0], 0.0F, 0.0F,   dx,
      0.0F, t[1], 0.0F,   dy,
      0.0F, 0.0F, t[2],   dz,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 scalev (f32 t, Vec3 v) noexcept {
    auto f = t - 1.0F;
    auto x = v[0];
    auto y = v[1];
    auto z = v[2];
    auto fx = f * x;
    auto fy = f * y;
    auto fz = f * z;
    auto fxx = fx * x;
    auto fxy = fx * y;
    auto fxz = fx * z;
    auto fyy = fy * y;
    auto fyz = fy * z;
    auto fzz = fz * z;
    return {
      fxx+1,   fxy,   fxz, 0.0F,
        fxy, fyy+1,   fyz, 0.0F,
        fxz,   fyz, fzz+1, 0.0F,
       0.0F,  0.0F,  0.0F, 1.0F,
    };
  }
  inline static Mat4 scalevp (f32 t, Vec3 v, Vec3 p) noexcept {
    auto f = t - 1.0F;
    auto x = v[0];
    auto y = v[1];
    auto z = v[2];
    auto w = -x*p[0] - y*p[1] - z*p[2];
    auto fx = f * x;
    auto fy = f * y;
    auto fz = f * z;
    auto fxx = fx * x;
    auto fxy = fx * y;
    auto fxz = fx * z;
    auto fxw = fx * w;
    auto fyy = fy * y;
    auto fyz = fy * z;
    auto fyw = fy * w;
    auto fzz = fz * z;
    auto fzw = fz * w;
    return {
      fxx+1,   fxy,   fxz,  fxw,
        fxy, fyy+1,   fyz,  fyw,
        fxz,   fyz, fzz+1,  fzw,
       0.0F,  0.0F,  0.0F, 1.0F,
    };
  }
  inline static Mat4 skewx (Vec3 d) noexcept {
    auto n = d / d[0];
    return {
      1.0F, 0.0F, 0.0F, 0.0F,
      n[1], 1.0F, 0.0F, 0.0F,
      n[2], 0.0F, 1.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 skewy (Vec3 d) noexcept {
    auto n = d / d[1];
    return {
      1.0F, n[0], 0.0F, 0.0F,
      0.0F, 1.0F, 0.0F, 0.0F,
      0.0F, n[2], 1.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 skewz (Vec3 d) noexcept {
    auto n = d / d[2];
    return {
      1.0F, 0.0F, n[0], 0.0F,
      0.0F, 1.0F, n[1], 0.0F,
      0.0F, 0.0F, 1.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 skewxp (Vec3 d, f32 p) noexcept {
    auto n = d / d[0];
    auto dy = -p * n[1];
    auto dz = -p * n[2];
    return {
      1.0F, 0.0F, 0.0F, 0.0F,
      n[1], 1.0F, 0.0F,   dy,
      n[2], 0.0F, 1.0F,   dz,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 skewyp (Vec3 d, f32 p) noexcept {
    auto n = d / d[1];
    auto dx = -p * n[0];
    auto dz = -p * n[2];
    return {
      1.0F, n[0], 0.0F,   dx,
      0.0F, 1.0F, 0.0F, 0.0F,
      0.0F, n[2], 1.0F,   dz,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 skewzp (Vec3 d, f32 p) noexcept {
    auto n = d / d[2];
    auto dx = -p * n[0];
    auto dy = -p * n[1];
    return {
      1.0F, 0.0F, n[0],   dx,
      0.0F, 1.0F, n[1],   dy,
      0.0F, 0.0F, 1.0F, 0.0F,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 lookat (Vec3 s, Vec3 d, Vec3 u) noexcept {
    Vec3 z = s - d;
    z.normalize();
    Vec3 x = u ^ z;
    x.normalize();
    Vec3 y = z ^ x;
    y.normalize();
    auto dx = -s[0]*x[0] - s[1]*x[1] - s[2]*x[2];
    auto dy = -s[0]*y[0] - s[1]*y[1] - s[2]*y[2];
    auto dz = -s[0]*z[0] - s[1]*z[1] - s[2]*z[2];
    return {
      x[0], x[1], x[2],   dx,
      y[0], y[1], y[2],   dy,
      z[0], z[1], z[2],   dz,
      0.0F, 0.0F, 0.0F, 1.0F,
    };
  }
  inline static Mat4 orthogonal (const Box3& frustum) noexcept {
    auto dx = frustum.X() - frustum.x();
    auto dy = frustum.Y() - frustum.y();
    auto dz = frustum.Z() - frustum.z();
    auto sx = frustum.X() + frustum.x();
    auto sy = frustum.Y() + frustum.y();
    auto sz = frustum.Z() + frustum.z();
    return {
      2/dx, 0.0F, 0.0F, sx/dx,
      0.0F, 2/dy, 0.0F, sy/dy,
      0.0F, 0.0F, 2/dz, sz/dz,
      0.0F, 0.0F, 0.0F,  1.0F,
    };
  }
  inline static Mat4 perspective (f32 fovy, f32 aspect, f32 near, f32 far) noexcept {
    auto i = 1.0F / (near-far);
    auto a = 1.0F / tanf(0.5F*fovy);
    auto b = a / aspect;
    auto c = i * (far+near);
    auto d = -2.0F * i * far * near;
    return {
         b, 0.0F,  0.0F, 0.0F,
      0.0F,    a,  0.0F, 0.0F,
      0.0F, 0.0F,     c,    d,
      0.0F, 0.0F, -1.0F, 0.0F,
    };
  }
};


inline Vec4 operator* (Vec4 v, const Mat4& m) noexcept {
  auto x = v.x();
  auto y = v.y();
  auto z = v.z();
  auto w = v.w();
  return {
    x*m.s00 + y*m.s01 + z*m.s02 + w*m.s03,
    x*m.s10 + y*m.s11 + z*m.s12 + w*m.s13,
    x*m.s20 + y*m.s21 + z*m.s22 + w*m.s23,
    x*m.s30 + y*m.s31 + z*m.s32 + w*m.s33,
  };
}


CLOSE_JLIB_NS